#include "cvcamera.h"

using namespace bottracker;

CvCamera::CvCamera(CvSize FrameSize, CvSize InnerVertexes)
{
    pm_ImageSize        = FrameSize;
    pm_BoardSize        = InnerVertexes;
    pm_CalibPoints      = pm_BoardSize.width*pm_BoardSize.height;
    pm_Grabbed          = 0;                //Wszytkie potrzebne macierze i struktury
    pm_ObjectPoints     = 0;                //zostaną utworzone w odpowiednich funkcjach
    pm_ImagePoints      = 0;
    pm_CornersFound     = 0;
    pm_Patterns.reserve(NFRAMES);           //rezerwuje miejsce na 10 obrazów, w razie potrzeby miejsce się powiększy
    pm_IntrinsicMat     = cvCreateMat( 3, 3, CV_32FC1 );
    pm_DistCoeffs       = cvCreateMat( 4, 1, CV_32FC1 );
    cvZero( pm_IntrinsicMat );
    cvZero( pm_DistCoeffs );

}
CvCamera::~CvCamera()
{
    cvReleaseMat( &pm_IntrinsicMat );
    cvReleaseMat( &pm_DistCoeffs );
    cvReleaseMat( &pm_ObjectPoints );
    cvReleaseMat( &pm_ImagePoints );
    for( unsigned int i=0;i<pm_Patterns.size();i++ )
        cvReleaseImage( &pm_Patterns[i] );
}

inline void CvCamera::init_object_points(int SquareSize )
{
    pm_ObjectPoints = cvCreateMat( 1,                                 //Dopiero tutaj alokowana jest macierz wzorca
                                   pm_Grabbed*pm_BoardSize.width*pm_BoardSize.height,
                                   CV_32FC3 );
    CvPoint3D32f* ptr3D = (CvPoint3D32f*)pm_ObjectPoints->data.fl;      //Wskażnik na dane float w strukturze CvMat
    for( int k=0;k<pm_Grabbed;k++ )                                   //Dla macierzy [[abc][def][ghi]]
        for( int i=0;i<pm_BoardSize.height;i++ )                       //kolejność wierzchołków jest następująca
            for( int j=0;j<pm_BoardSize.width;j++ )                  //[gdahebifc]
                *ptr3D++ = cvPoint3D32f(i*SquareSize, j*SquareSize, 0);
}

// TODO (bolek#1#): Zapisywanie wszystkich potrzebnych i nie rzeczybool CvCamera::save_data( const char* FileName )
{
    CvFileStorage* fs = cvOpenFileStorage( FileName, 0, CV_STORAGE_WRITE );
    if(!fs)
    {
        cerr<<"Error opening " << FileName << " for writing";
        return false;
    }

    time_t t;
    time( &t );
    struct tm *t2 = localtime( &t );
    char buf[1024];
    strftime( buf, sizeof(buf)-1, "%c", t2 );
    cvWriteString( fs, "calibration_time", buf );
    cvWrite( fs, "camera_matrix", pm_IntrinsicMat );
    cvWrite( fs, "distortion_coeffs", pm_DistCoeffs );
    if( pm_ObjectPoints )
        cvWrite( fs, "object_points", pm_ObjectPoints );
    if( pm_ImagePoints )
        cvWrite( fs, "image_points", pm_ImagePoints );
    cvReleaseFileStorage( &fs );
    return true;
}
bool CvCamera::get_frame( IplImage* frame )
{
    if(pm_Grabbed > NFRAMES)
        pm_Patterns.resize(pm_Grabbed*NFRAMES);               //alternatywa do realloc w c++
    pm_Patterns[pm_Grabbed++] = cvCloneImage( frame );
    return true;
}

bool CvCamera::calibrate( int SquareSize )
{
    int success = 0;
    pm_ImagePoints = cvCreateMat( 1,                                 //Dopiero tutaj alokowana jest punktów 2D wzorca
                                  pm_Grabbed*pm_BoardSize.width*pm_BoardSize.height,
                                  CV_32FC2 );
    cvZero( pm_ImagePoints );
    IplImage* subCorners = cvCreateImage( pm_ImageSize,
                                          IPL_DEPTH_8U,
                                          1);               //Obrazek w skali szarości potrzebny funcji cvFindCornerSubPix

    pm_CornersFound = cvCreateMat ( 1,
                                    pm_Grabbed,
                                    CV_32SC1 );

    CvPoint2D32f* ptr2D = (CvPoint2D32f*)pm_ImagePoints->data.fl;
    int* ptr = (int*)pm_CornersFound->data.i;

    init_object_points( SquareSize );

    for( int i=0;i<pm_Grabbed;i++ )
    {

        success = cvFindChessboardCorners( pm_Patterns[i], 	        	//Obrazek z wzorcem
                                           pm_BoardSize,
		                                   ptr2D+pm_CalibPoints*i,      //współrzędne znalezionych wierzchołkó
		                                   ptr+i,		                //ilość wierzchołków znaleziona w każdej iteracji
		                                   CV_CALIB_CB_FILTER_QUADS );
        if(success)
		{
			cvCvtColor( pm_Patterns[i], subCorners, CV_BGR2GRAY );		//Konwesja do obrazka w odcieniach szarości
			cvFindCornerSubPix( subCorners,
	                		    ptr2D+pm_CalibPoints*i,
                                *(ptr+i),				                //ilość wierzchołków
	                    		cvSize( 5,5 ),
	                    		cvSize( -1,-1 ),
	                    		cvTermCriteria(CV_TERMCRIT_ITER, 100, 0.1) );
		}
		else
            return false;
    }
    cvCalibrateCamera2( pm_ObjectPoints,
	                    pm_ImagePoints,
	                    pm_CornersFound,
	                    pm_ImageSize,
                        pm_IntrinsicMat,
	                    pm_DistCoeffs,
	                    0,
	                    0);

    return true;
}
